#include <iostream>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <opencv2/opencv.hpp>

#include <thread>
#include <termios.h>
#include "utm_transformer.h"
// system include.
#include <pthread.h>
#include <thread>
#include <iostream>
#include <fstream>
#include <string.h>
// apply include.
#include <mutex>
#include <cstdlib> 
#include "control.pb.h"
#include "localization.pb.h"
#include <google/protobuf/io/zero_copy_stream_impl.h>
#include <google/protobuf/text_format.h>

#include <ecal/ecal.h>
#include <ecal/msg/protobuf/publisher.h>
#include <ecal/msg/protobuf/subscriber.h>

#include "../../../include/api/bulidMap/bulidMapApi.h"

#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <regex>

#include <unordered_set>
#include "utm_transformer.h"

#include "spdlog/sinks/basic_file_sink.h" //基本文件记录器
#include "spdlog/spdlog.h"
#include "spdlog/sinks/stdout_color_sinks.h" //stderr 记录器对象

namespace std {
    template<> struct hash<vector<double>> {
        size_t operator()(const vector<double>& v) const {
            size_t seed = 0;
            for (double d : v) {
                // Convert double to its integer representation
                size_t hashValue = std::hash<double>{}(d);
                seed ^= hashValue + 0x9e3779b9 + (seed << 6) + (seed >> 2);
            }
            return seed;
        }
    };
}



C_bulidMapApi::C_bulidMapApi(/* args */)
{
    car_width = 40.0;
    car_height = 20.0;
    meter_to_centi=100.0; // 米到厘
    coordinate_offset_x=0;
    coordinate_offset_y=0;
    tolerance=10.0;
    map_width=400;
    map_height=400;
    dis_threshold=20; // 距离阈值
    polygon_feasible=false;
    lat_degree;
    lon_degree;
    // std::vector<std::vector<double>> car_coordinate; // 储存坐标信息的二维数组，n行2列，储存n个横纵坐标

    max_abs_negative = {0.0, 0.0}; // 保存图像的最大宽度
    max_abs_positive= {0.0, 0.0}; 

    first_point_x= -std::numeric_limits<double>::infinity(); // 使用numeric_limits方法将定义的变量设置极值
    first_point_y= -std::numeric_limits<double>::infinity();
}

C_bulidMapApi::~C_bulidMapApi()
{
}



/***********************************************
 * @author: Guowanda
 * @date:   23/10
 * @brief:  把读取到的经纬度数据转换成标准格式，例如 310.11231 -> 31.011231
 * @var:    经纬度数据
 * @return: 经纬度数据
 ***********************************************/
double C_bulidMapApi::ConvertToDegrees(const std::string& in_data1, const std::string& in_data2) 
{
    //将度分秒形式的地理坐标数据转换为小数度形式，以便后续地理计算
    int len_data1 = in_data1.length();
    std::string str_data2 = in_data2;
    while (str_data2.length() < 5) {
        str_data2 = "0" + str_data2;
    }  
    
    int temp_data = std::stoi(in_data1);
    int symbol = 1;
    if (temp_data < 0) {
        symbol = -1;
    }
    
    int degree = temp_data / 100;
    std::string str_decimal = in_data1.substr(len_data1 - 2, 2) + str_data2;
    double f_degree = std::stoi(str_decimal) / 60.0 / 100000.0;
    
    double result;
    if (symbol > 0) {
        result = degree + f_degree;
    } else {
        result = degree - f_degree;
    }
    
    return result;
}


/***********************************************
 * @author: Guowanda
 * @date:   23/10
 * @brief:  把读取到的经纬度数据转换成标准格式，例如 310.11231 -> 31.011231
 * @var:    经纬度数据
 * @return: 经纬度数据
 ***********************************************/
double C_bulidMapApi::ConvertToDegrees_double(double& in_data) 
{
    //将度分秒形式的地理坐标数据转换为小数度形式，以便后续地理计算
    double temp_data = in_data;
    int symbol = 1;
    if (temp_data < 0) {
        symbol = -1;
    }
    temp_data=temp_data / 100;
    int integerPart = static_cast<int>(temp_data);
    double decimalPart = temp_data - integerPart;
    
    int degree = integerPart;
    double f_degree = decimalPart / 60.0 *100;
    
    double result;
    if (symbol > 0) {
        result = degree + f_degree;
    } else {
        result = degree - f_degree;
    }
    
    return result;
}


/***********************************************
 * @author: Guowanda
 * @date:   23/10
 * @brief:  绘制箭头的线段 代表朝向，仅仅用于展示建图过程
 * @var:    先后两个位置点、图像的数据
 * @return: void
 ***********************************************/
void C_bulidMapApi::draw_arrow(cv::Point start_point, cv::Point end_point, cv::Mat& image) 
{
    // 绘制箭头的线段
    cv::line(image, start_point, end_point, cv::Scalar(0, 0, 255), 2);

    // 绘制箭头头部
    int arrow_size = 15;
    int dx = end_point.x - start_point.x;
    int dy = end_point.y - start_point.y;
    double angle = std::atan2(static_cast<double>(dy), static_cast<double>(dx));
    double length = cv::norm(cv::Point(dx, dy));
    double delta_x = arrow_size * dx / length;
    double delta_y = arrow_size * dy / length;

    // 计算箭头头部的两个点
    cv::Point arrow_point1(
        static_cast<int>(end_point.x - delta_x - delta_y * std::sin(angle)),
        static_cast<int>(end_point.y - delta_y + delta_x * std::sin(angle))
    );
    cv::Point arrow_point2(
        static_cast<int>(end_point.x - delta_x + delta_y * std::sin(angle)),
        static_cast<int>(end_point.y - delta_y - delta_x * std::sin(angle))
    );

    // 绘制箭头头部
    cv::line(image, end_point, arrow_point1, cv::Scalar(0, 0, 255), 2);
    cv::line(image, end_point, arrow_point2, cv::Scalar(0, 0, 255), 2);
}


/***********************************************
 * @author: Guowanda
 * @date:   23/10
 * @brief:  绘制旋转矩形,用于代表小车，实时显示小车动态以及最后展示
 * @var:    先后两个位置点、小车的长宽、图像的数据
 * @return: void
 ***********************************************/
void C_bulidMapApi::draw_rotated_rect(cv::Point point1, cv::Point point2, double car_width, double car_height, cv::Mat& image) 
{
    double angle = std::atan2(point2.y - point1.y, point2.x - point1.x); // 通过两个点计算方向
    double rect_width = car_width;
    double rect_height = car_height;

    double center_x = (point1.x + point2.x) / 2.0; // 计算小车中心位置
    double center_y = (point1.y + point2.y) / 2.0;

    double top_left_x = center_x - rect_width / 2.0; // 绘制小车顶点
    double top_left_y = center_y - rect_height / 2.0;
    double bottom_right_x = center_x + rect_width / 2.0;
    double bottom_right_y = center_y + rect_height / 2.0;

    std::vector<cv::Point2f> rotated_rect_points = {
        cv::Point2f(top_left_x, top_left_y),
        cv::Point2f(bottom_right_x, top_left_y),
        cv::Point2f(bottom_right_x, bottom_right_y),
        cv::Point2f(top_left_x, bottom_right_y)
    };

    double cos_angle = std::cos(angle);
    double sin_angle = std::sin(angle);

    std::vector<cv::Point2f> rotated_rect;
    for (const cv::Point2f& point : rotated_rect_points) {
        double x = center_x + (point.x - center_x) * cos_angle - (point.y - center_y) * sin_angle;
        double y = center_y + (point.x - center_x) * sin_angle + (point.y - center_y) * cos_angle;
        rotated_rect.emplace_back(cv::Point2f(x, y));
    }

    for (size_t i = 0; i < rotated_rect.size(); ++i) {
        cv::line(image, rotated_rect[i], rotated_rect[(i + 1) % 4], cv::Scalar(255, 0, 0), 2);
    }

    double point2_x = center_x + rect_width * cos_angle;
    double point2_y = center_y + rect_width * sin_angle;

    draw_arrow(cv::Point(center_x, center_y), cv::Point(point2_x, point2_y), image);
}

/***********************************************
 * @author: Guowanda
 * @date:   23/10
 * @brief:  多边形膨胀，用于对地图整体尺寸进行缩放，绘制地图安全区域边界
 * @var:    多边形的顶点、膨胀系数
 * @return: 膨胀后的多边形顶点
 ***********************************************/
mpolygon_t C_bulidMapApi::expand_polygon(const polygon_t& polygon, const double distance) 
{
    mpolygon_t scaled_polygon;
    // DistanceStrategy is symmetric; want straight lines (no curves)
    bg::strategy::buffer::distance_symmetric<double>
      distance_strategy{distance};

    // SideStrategy is straight; grow equally on all sides
    bg::strategy::buffer::side_straight side_strategy;

    // JoinStrategy is miter; 1.0 as limit; Sharp (not rounded) joins
    bg::strategy::buffer::join_miter join_strategy;

    // EndStrategy is flat; flat (not rounded) ends
    bg::strategy::buffer::end_flat end_strategy;

    // PointStrategy is square; squares, not circles, if poly is just a point
    bg::strategy::buffer::point_square point_strategy;

    bg::buffer(polygon, scaled_polygon,
                            distance_strategy,
                            side_strategy,
                            join_strategy,
                            end_strategy,
                            point_strategy);

    // return scaled polygon offset by supplied distance
    return scaled_polygon;
}


/***********************************************
 * @author: Guowanda
 * @date:   23/10
 * @brief:  计算两点之间的距离
 * @var:    第一个点不是数组，下一个目标点
 * @return: 两个点之间的距离
 ***********************************************/
double C_bulidMapApi::calculateDistance(const std::vector<double>& point1, const std::vector<double>& point2) 
{
    double distance = 0.0;
    for (size_t i = 0; i < point1.size(); ++i) {
        double diff = point1[i] - point2[i];
        distance += diff * diff;
    }
    return std::sqrt(distance); // 计算开平方根
}


/***********************************************
 * @author: Guowanda
 * @date:   23/10
 * @brief:  删除重复行读取数据，对每一行的数据进行判断去重
 * @var:    二维数组
 * @return: 二维数组
 ***********************************************/
std::vector<std::vector<double>> C_bulidMapApi::removeDuplicateRows_read_data(const std::vector<std::vector<double>>& input)
{
    std::vector<std::vector<double>> uniqueVector;
    std::unordered_set<std::vector<double>> uniqueSet;

    for (const auto &row : input) {
        if (uniqueSet.find(row) == uniqueSet.end()) {
            uniqueSet.insert(row);
            uniqueVector.push_back(row);
        }
    }
    return uniqueVector;
} 


/***********************************************
 * @author: tianxiaohua
 * @date:   23/10
 * @brief:  通过一个vector buf来转存回调函数中接收到的数据，形成一个fifo结构。防止数据丢失处理不过来，
 * @var:    经纬度数据
 * @return: void
 ***********************************************/
std::vector<COORDINATE> DataBuf;
void C_bulidMapApi::fifoSaveDate(double lat_degree, double lon_degree)
{
    extern std::vector<COORDINATE> DataBuf;
    COORDINATE tempCoordinate;
    // printf("经度:%f\n", lat_degree);
    // printf("纬度:%f\n", lon_degree);
    tempCoordinate.lat_degree = lat_degree;
    tempCoordinate.lon_degree = lon_degree;
    DataBuf.push_back(tempCoordinate);
} 


/***********************************************
 * @author: tianxiaohua
 * @date:   23/10
 * @brief:  获取fifo中转存的数据内容，按照顺序输出；
 * @var:    经纬度数据结构体
 * @return: 获取成功和失败的标志
 ***********************************************/
bool C_bulidMapApi::getFifoDate(COORDINATE &tempCoordinate)
{
    int vertorLen = DataBuf.size();
    if(vertorLen == 0){
        return false;
    }
    else{
        tempCoordinate = DataBuf[0];
        DataBuf.erase(DataBuf.begin());
        return true;
    }
    return false;
}


/***********************************************
 * @author: Guowanda
 * @date:   23/10
 * @brief:  线程函数，涉及按键触发建图线程启停
 * @var:    void
 * @return: void
 ***********************************************/
void C_bulidMapApi::keyboardThread() 
{
    char check_keys = ' '; 
    struct termios original_attr, new_attr; // 获取终端的属性
    tcgetattr(STDIN_FILENO, &original_attr);
    new_attr = original_attr;

    // 禁用终端的回显和行缓冲
    new_attr.c_lflag &= ~(ICANON | ECHO);
    tcsetattr(STDIN_FILENO, TCSANOW, &new_attr);

    std::cout << "Pressed key:s start, Pressed key: q or Esc quit" << std::endl;
    while (true) {
        char key;
        if (read(STDIN_FILENO, &key, 1) == 1) {
            if (key == 'q' || key == 'Q' || key ==27) {
                check_keys = 'q';
            } else if(key =='s' || key=='S'){
                check_keys='s';
                std::cout << "Pressed key: " << key << std::endl;
                COORDINATE tempCoordinate;
                eCAL::protobuf::CPublisher<MowerProtoPakg::bulidMapLocalization> *p_ecal_bulidMapMessage;
                eCAL::protobuf::CPublisher<MowerProtoPakg::bulidMapLocalization> ecal_bulidMapMessage("bulidMapMessage");
                p_ecal_bulidMapMessage = &ecal_bulidMapMessage;
                while(true)
                {
                    if(getFifoDate(tempCoordinate) == true){
                        bulidMap(tempCoordinate.lat_degree, tempCoordinate.lon_degree, p_ecal_bulidMapMessage);
                    }
                }
            }
        }
    }
}

/***********************************************
 * @author: Guowanda
 * @date:   23/10
 * @brief:  使用opencv的图片刷新显示建图过程
 * @var:    car_coordinate_copy：二位数字，储存N个坐标点
 * @return: void
 ***********************************************/
void C_bulidMapApi::DisplayBulidMap(std::vector<std::vector<double>> car_coordinate_copy)
{
    cv::Mat image(map_height,map_width , CV_8UC3, cv::Scalar(125, 125, 125)); // cv::Mat是OpenCV定义的用于表示任意维度的稠密数组，OpenCV使用它来存储和传递图像
    for (const auto& point : car_coordinate_copy) {
        int x = static_cast<int>(point[0]);
        int y = static_cast<int>(point[1]);
        cv::circle(image, cv::Point(x, y), 5, cv::Scalar(0, 0, 255), -1); // 画边界多边形顶点
    }

    size_t point_num = car_coordinate_copy.size();
    if (point_num>=2){
        cv::Point cvPoint1(car_coordinate_copy[point_num-2][0], car_coordinate_copy[point_num-2][1]);
        cv::Point cvPoint2(car_coordinate_copy[point_num-1][0], car_coordinate_copy[point_num-1][1]);
        draw_rotated_rect(cvPoint1, cvPoint2, car_width, car_height, image); //画小车
    }

    // 显示图像
    cv::imshow("Points", image);
    cv::waitKey(100);
}


void C_bulidMapApi::saveImage(cv::Mat &image, std::vector<cv::Point> &polygon_points_small, polygon_t &polygon_after_simplified)
{
    std::string filename_boundary_map = "../result/boundary_map.png";
    std::string filename_boundary_map_with_car = "../result/boundary_map_with_car.png";
    std::string filename_PolygonVertex = "../result/PolygonVertex.txt";

    // 使用cv::imwrite来保存图像
    bool success = cv::imwrite(filename_boundary_map, image);

    if (success) {
        std::cout << "图像保存成功！" << std::endl;
    } else {
        std::cout << "图像保存失败..." << std::endl;
    }
    /////////////////////////////////////////////////////////////////////////////////////
    std::vector<point_t> points;
    for (const auto &point : polygon_after_simplified.outer()) {
        points.push_back(point);
    }


    for (std::size_t i = 0; i < points.size() - 1; ++i) {
        cv::Point cvPoint1(points[i].x(), points[i].y());
        cv::Point cvPoint2(points[i + 1].x(), points[i + 1].y());

        draw_rotated_rect(cvPoint1, cvPoint2, car_width, car_height, image);
    }
    
    // 使用cv::imwrite来保存图像
    bool success_ = cv::imwrite(filename_boundary_map_with_car, image);

    if (success_) {
        std::cout << "图像保存成功！" << std::endl;
    } else {
        std::cout << "图像保存失败..." << std::endl;
    }
    /////////////////////////////////////////////////////////////////////////////////////
    std::ofstream outfile(filename_PolygonVertex, std::ios::out);

    if (!outfile.is_open()) {
        std::cerr << "无法打开文件，尝试创建并写入数据" << std::endl;

        // 创建新的文本文件并打开以进行写入操作
        outfile.open("PolygonVertex.txt", std::ios::out | std::ios::trunc);

        if (!outfile.is_open()) {
            std::cerr << "无法创建文件" << std::endl;
            exit(0);
        }
    }

    for (const cv::Point& point : polygon_points_small) { // 遍历 vector 中的点，将它们写入文件
        outfile << point.x << " " << point.y << std::endl;
    }

    outfile.close(); // 关闭文件

}

void ecal_sandMessage(double lat_degree, double lon_degree, double start_lat_degree, double start_lon_degree, eCAL::protobuf::CPublisher<MowerProtoPakg::bulidMapLocalization> *p_ecal_bulidMapMessage)
{
    // 把数据包装成protobuf
    MowerProtoPakg::bulidMapLocalization message;// 创建一个protobuf消息对象
    message.set_wordx(lat_degree);
    message.set_wordy(lon_degree);
    message.set_startx(start_lat_degree);
    message.set_starty(start_lon_degree);
    // Send the message
    p_ecal_bulidMapMessage->Send(message);
}

/***********************************************
 * @author: Guowanda
 * @date:   23/10
 * @brief:  创建地图算法，单独一个线程中调用
 * @var:    lat_degree：地图的横坐标； lon_degree：地图的纵坐标
 * @return: void
 ***********************************************/
void C_bulidMapApi::bulidMap(double lat_degree, double lon_degree, eCAL::protobuf::CPublisher<MowerProtoPakg::bulidMapLocalization> *p_ecal_bulidMapMessage)
{   
    /////处理数据////////////////////////////////////////////////////////       
    double word_X = 0;
    double word_Y = 0;
    std::vector<double> temp_row;
    
    std::pair<double, double> coordinate = from_latlon(lat_degree, lon_degree); // 经纬度数据转换为世界坐标系 
    
    // 设置第一个坐标 Set the first coordinate
    if (std::isinf(first_point_x) && first_point_x < 0) {
        first_point_x=coordinate.first;
        first_point_y=coordinate.second;
    } 
    word_X = (coordinate.first - first_point_x) * meter_to_centi; // 转换成厘米单位的相对位置坐标点
    word_Y = (coordinate.second - first_point_y) * meter_to_centi;
    ecal_sandMessage(word_X, word_Y, first_point_x, first_point_y, p_ecal_bulidMapMessage);
    temp_row.push_back(word_X);
    temp_row.push_back(word_Y);
    car_coordinate.push_back(temp_row);
    spdlog::info("bulidMap: {} {} {}",temp_row.size(), word_X, word_Y);
    std::vector<std::vector<double>> car_coordinate_array = removeDuplicateRows_read_data(car_coordinate); // 删除重复行读取数据

    /////////////////////////////////////////////////////////////
    if (max_abs_negative[0]>temp_row[0]){ // 对每次输入的数据都进行比较, 保存图像的最大宽度
        max_abs_negative[0]=temp_row[0];
    }
    if (max_abs_negative[1]>temp_row[1]){
        max_abs_negative[1]=temp_row[1];
    }
    if (max_abs_positive[0]<temp_row[0]){
        max_abs_positive[0]=temp_row[0];
    }  
    if (max_abs_positive[1]<temp_row[1]){
        max_abs_positive[1]=temp_row[1];
    }

    coordinate_offset_x=-max_abs_negative[0]*1.2; // 对基础图片进行大小的拓展
    coordinate_offset_y=-max_abs_negative[1]*1.2;

    if (coordinate_offset_x+max_abs_positive[0]>map_width){// 对地图图片宽度拓展
        map_width=(coordinate_offset_x+max_abs_positive[0])*1.2;
    } 
    if (coordinate_offset_y+max_abs_positive[1]>map_height){
        map_height=(coordinate_offset_y+max_abs_positive[1])*1.2;
    }
    
    std::vector<std::vector<double>> car_coordinate_copy = car_coordinate_array;
    for (auto& row : car_coordinate_copy) { // 整体移动坐标
        // 对第一列和第二列进行加法操作
        row[0] += coordinate_offset_x;
        row[1] += coordinate_offset_y;
    }

    const std::vector<double>& first_point = car_coordinate_array[0];
    const std::vector<double>& last_point = car_coordinate_array[car_coordinate_array.size() - 1];
    double distance = calculateDistance(first_point, last_point); // 计算第一个点到最后一个点的距离，用于确定地图是否可以封闭
    
    if (distance<=dis_threshold &&  car_coordinate_array.size()>50){ // 判断当前点和最后一个点距离是不是足够小，产生封闭图像，单位厘米。如果RTK坐标的坐标点数量大于50
        polygon_feasible=true; // 如果多边形点太少是不可以用的
    } 

    if (polygon_feasible==false){ // 建图的点和距离还不够建图的情况下继续采集数据
        DisplayBulidMap(car_coordinate_copy); // 实时显示小车位置朝向
    } 
    else{ // 如果判断多边形已经封闭，则完成建图，完成建图后会对图像做处理操作
        using bg::get;
        polygon_t rect;
        for (const auto& coord : car_coordinate_copy) {
            bg::append(rect.outer(), point_t(coord[0], coord[1]));
        }
        bg::correct(rect);
        // 输出多边形中的点数量
        std::cout << "Number of points in the polygon: " << rect.outer().size() << std::endl;

        polygon_t simplified_poly_1; // to hold new geometry
        bg::simplify(rect,simplified_poly_1,tolerance); // epsilon value

        // trim the polygon 休整多边形
        mpolygon_t shrink_polygon_small = expand_polygon(simplified_poly_1, -0.5);

        cv::Mat image(map_height,map_width , CV_8UC3, cv::Scalar(125, 125, 125)); // cv::Mat是OpenCV定义的用于表示任意维度的稠密数组，OpenCV使用它来存储和传递图像
        std::vector<cv::Point> polygon_points_small;

        for (const auto& polygon : shrink_polygon_small) {
            for (const auto& point : polygon.outer()) {
                //cv::Point cv_point(static_cast<int>(point.x()), static_cast<int>(point.y()));
                //cv::circle(image, cv_point,8, cv::Scalar(0, 0, 0), -1); // Draw a small circle for each point
                polygon_points_small.push_back(cv::Point(point.x(), point.y()));
            }
        }
        for (const auto& point : polygon_points_small) { 
            cv::circle(image, point, 10, cv::Scalar(0, 255, 0), -1); // Draw a small circle for each point
        }                       
        
        polygon_t polygon_after_simplified;
        bg::clear(polygon_after_simplified);

        for (auto const &sub_polygon : shrink_polygon_small) {
            for (auto const &point : sub_polygon.outer()) {
                //cv::Point cv_point(static_cast<int>(point.x()), static_cast<int>(point.y()));
                //draw_rotated_rect(point1, point2, car_width, car_height, image);
                bg::append(polygon_after_simplified, point);
            }
        }

        /////////////////////////////////////////////////////////////////////////////////////
        mpolygon_t shrink_polygon_out_1 = expand_polygon(polygon_after_simplified, car_height/2);

        std::vector<cv::Point> polygon_points_out_1;

        for (const auto& polygon : shrink_polygon_out_1) {
            for (const auto& point : polygon.outer()) {
                cv::Point cv_point(static_cast<int>(point.x()), static_cast<int>(point.y()));
                cv::circle(image, cv_point,3, cv::Scalar(0, 0, 0), -1); // Draw a small circle for each point
                polygon_points_out_1.push_back(cv::Point(point.x(), point.y()));
            }
        }

        std::vector<std::vector<cv::Point>> contours_out_1;
        contours_out_1.push_back(polygon_points_out_1);
        cv::fillPoly(image, contours_out_1, cv::Scalar(0, 0, 0));
        /////////////////////////////////////////////////////////////////////////////////////
        mpolygon_t shrink_polygon_inner_1 = expand_polygon(simplified_poly_1, -car_height/2);

        std::vector<cv::Point> polygon_points_inner_1;

        for (const auto& polygon : shrink_polygon_inner_1) {
            for (const auto& point : polygon.outer()) {
                cv::Point cv_point(static_cast<int>(point.x()), static_cast<int>(point.y()));
                cv::circle(image, cv_point,3, cv::Scalar(0, 0, 0), -1); // Draw a small circle for each point
                polygon_points_inner_1.push_back(cv::Point(point.x(), point.y()));
            }
        }

        std::vector<std::vector<cv::Point>> contours_inner_1;
        contours_inner_1.push_back(polygon_points_inner_1);
        cv::fillPoly(image, contours_inner_1, cv::Scalar(125, 125, 125));
        /////////////////////////////////////////////////////////////////////////////////////
        mpolygon_t shrink_polygon_inner_2 = expand_polygon(simplified_poly_1, -car_height);

        std::vector<cv::Point> polygon_points_inner_2;

        for (const auto& polygon : shrink_polygon_inner_2) {
            for (const auto& point : polygon.outer()) {
                cv::Point cv_point(static_cast<int>(point.x()), static_cast<int>(point.y()));
                //cv::circle(image, cv_point,3, cv::Scalar(0, 0, 0), -1); // Draw a small circle for each point
                polygon_points_inner_2.push_back(cv::Point(point.x(), point.y()));
            }
        }

        std::vector<std::vector<cv::Point>> contours_inner_2;
        contours_inner_2.push_back(polygon_points_inner_2);
        cv::fillPoly(image, contours_inner_2, cv::Scalar(255, 255, 255));
        /////////////////////////////////////////////////////////////////////////////////////
        saveImage(image, polygon_points_small, polygon_after_simplified);
        cv::imshow("Points", image);
        cv::waitKey(200);
    }
}


/***********************************************
 * @author: Guowanda
 * @date:   23/10
 * @brief:  回调函数，通过ecal订阅获取坐标的报文进行回调
 * @var:    lat_degree：地图的横坐标； lon_degree：地图的纵坐标
 * @return: void
 ***********************************************/
void C_bulidMapApi::Callback(const MowerProtoPakg::localizationGNGGA& msg) 
{
    printf("time:%f\n", msg.time());
    fifoSaveDate(msg.latitude(), msg.longitude());
}


void C_bulidMapApi::initBulidMap(void)
{
    // std::thread keyboard_thread(g_bulidMap.keyboardThread);
    std::thread thread = std::thread(&C_bulidMapApi::keyboardThread, this); // 只能这么写，不这么写会报错

    eCAL::protobuf::CSubscriber<MowerProtoPakg::localizationGNGGA> subscriber("hello_world_python_protobuf_topic");
    subscriber.AddReceiveCallback(std::bind(&Callback, std::placeholders::_2));
    while (eCAL::Ok()) {
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    }
    eCAL::Finalize();
}

// int main(int argc, char *argv[]) 
// {
//     C_bulidMapApi *g_bulidMap = new C_bulidMapApi();
//     eCAL::Initialize(argc, argv, "GNGGA Publisher");

//     g_bulidMap->initBulidMap();
//     while(true){
//         usleep(1000000);
//     }
// }
